#include "talker.hpp"

namespace mars_rover
{
  namespace talker
  {
    TalkerNode::TalkerNode(const rclcpp::NodeOptions &options) : Node("talker_node", options)
    {
      std::cout << "start..." << std::endl;
      std::string dataset_dir = this->declare_parameter<std::string>("dataset_dir", "/");
      source_frame_ = this->declare_parameter<std::string>("source_frame", "source_frame");
      target_frame_ = this->declare_parameter<std::string>("target_frame", "target_frame");
      image_dir_ = this->declare_parameter<std::string>("image_dir", "image.png");
      image_dir_ = dataset_dir + image_dir_;
      pcd_dir_ = this->declare_parameter<std::string>("pcd_dir", "map.pcd");
      pcd_dir_ = dataset_dir + pcd_dir_;
      transform_ = this->declare_parameter<std::vector<double>>("transform", {});
      pose_data_ = this->declare_parameter<std::vector<double>>("pose", {});

      if(!init()){
        std::cout << "init fail, exit." << std::endl;
        exit(-1);
      }
      std::cout << "init done!" << std::endl;
      static_tf_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);
      pointcloud_publisher_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("pointcloud_raw", 1);
      image_publisher_ = this->create_publisher<sensor_msgs::msg::Image>("image_raw", 1);
      pose_publisher_ = this->create_publisher<geometry_msgs::msg::PoseStamped>("pose_raw", 1);
      pub_thread_.reset(new std::thread(&TalkerNode::run, this));
    }


    bool TalkerNode::init(){
      // load image
      std::cout << "init..." << std::endl;
      cv::Mat img;
      img = cv::imread(image_dir_, cv::IMREAD_UNCHANGED);
      cv_bridge::CvImage cv_image;
      cv_image = cv_bridge::CvImage(image_.header, sensor_msgs::image_encodings::BGR8, img);
      cv_image.toImageMsg(image_);
      image_.header.frame_id = target_frame_;
      std::cout << "load image." << std::endl;
      // load pcd
      pcl::PointCloud<pcl::PointXYZI>::Ptr map;
      map.reset(new pcl::PointCloud<pcl::PointXYZI>());
      pcl::io::loadPCDFile(pcd_dir_, *map);
      pcl::toROSMsg(*map, pointcloud_);
      pointcloud_.header.frame_id = target_frame_;
      std::cout << "load pointcloud." << std::endl;
      // load tf
      transform_stamp_.header.stamp = this->now();
      transform_stamp_.header.frame_id = source_frame_;
      transform_stamp_.child_frame_id = target_frame_;
      transform_stamp_.transform.translation.x = transform_[0];
      transform_stamp_.transform.translation.y = transform_[1];
      transform_stamp_.transform.translation.z = transform_[2];
      tf2::Quaternion q;
      q.setRPY(transform_[3], transform_[4], transform_[5]);
      transform_stamp_.transform.rotation = tf2::toMsg(q);
      std::cout << "load tf." << std::endl;
      // load pose
      pose_.pose.position.x = pose_data_[0];
      pose_.pose.position.y = pose_data_[1];
      pose_.pose.position.z = pose_data_[2];
      tf2::Quaternion q1;
      q1.setRPY(pose_data_[3], pose_data_[4], pose_data_[5]);
      pose_.pose.orientation = tf2::toMsg(q1);
      std::cout << "load pose." << std::endl;
      std::cout << "init done." << std::endl;
      return true;
    }


    void TalkerNode::run(){
      std::cout << "run..." << std::endl;
      rclcpp::Rate rate(10.0);
      while (rclcpp::ok()){
        /* code */
        transform_stamp_.header.stamp = this->now();
        static_tf_broadcaster_->sendTransform(transform_stamp_);
        pointcloud_.header.stamp = this->now();
        pointcloud_publisher_->publish(pointcloud_);
        image_.header.stamp = this->now();
        image_publisher_->publish(image_);
        pose_.header.stamp = this->now();
        pose_publisher_->publish(pose_);
        rate.sleep();
      }
    }

  } // namespace talker
} // namespace mars_rover

RCLCPP_COMPONENTS_REGISTER_NODE(mars_rover::talker::TalkerNode)
